#ifndef INC_5_FINAL_ASTARSEARCH_H
#define INC_5_FINAL_ASTARSEARCH_H

#include "cmath"
#include <array>
#include <chrono>
#include <cstring>
#include <iostream>
#include <queue>
#include <set>
#include <stack>
#include <tuple>
#include <vector>
#include <utility>

#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;

typedef pair<int, int> Pair;
typedef vector<vector<int>> Vec2int;

class AStar {
private:
    Mat binaryObstacleMap;
    Mat ObstacleMap;
    Mat rawMap;
    int ROW;
    int COL;
    Pair StartPoint;
    Pair EndPoint;
    stack<Pair> Path;
    string mapPath;
    vector<vector <int>> vecGrid;

public:

    struct cell {
        Pair nodeParent;
        double cost_f, cost_g, cost_h;
        cell() :
            nodeParent(-1, -1), cost_f(-1), cost_g(-1), cost_h(-1) {}
    };


    AStar();
    void setStartPoint(int x, int y);
    void setEndPoint(int x, int y);
    void aStarSearch();
    static Pair cor2ArrayPos(Pair corPos);
    static Pair array2CorPos(Pair arrayPos);
    bool isValid(const vector<vector <int>>& grid, const Pair& point) const;
    bool isUnBlocked(const vector<vector <int>>& grid, const Pair& point) const;
    bool isDestination(const Pair& position);
    static double calculateHValue(const Pair& src, const Pair& dest);
    void tracePath(const vector<vector <cell>>& cellDetails);
    void drawPath();
    Mat readMap(const string& filename);
    void setMapName(const string& filename);
    Mat getBinaryEdgeMap();
    static vector<vector <int>> mat2Vector(Mat& src, int m, int n);
    void openLidar(Pair pos);
    vector<vector <int>> getGridMap();
    static pair<double, double> getThetaRange(Pair point_0, Pair point_d);
    static vector<pair<double, double>> mergeInterval(const pair<double, double>& newRange,
        vector<pair<double, double>>& intervals);
    static bool checkInterval(const pair<double, double>& newRange,
        vector<pair<double, double>>& intervals);
    Vec2int getVector2intMap();

    Mat vec2RawMap();

    Vec2int getBoundaryGrid(Vec2int& raw) const;

};

#endif //INC_5_FINAL_ASTARSEARCH_H
